/**
 * @file   navigator.cpp
 * @brief  导航模块主进程
 * @encode UTF-8
 * @log    date        author      notes
 *         2021.09.01  Jimmy       first version
 */
#include <iostream>
#include "navigator.h"
#include <functional>
#include "simulation/simulation.h"
#include "inner_libs/usr/include/debug_log/log.h"
#include "inner_libs/usr/include/utils/date_time.h"

/**
 * @brief  Construct a new Navigator:: Navigator object
 * @author Jimmy 
 * @date   2021.09.01
 */
Navigator::Navigator()
{
    log_shell::LOG_INFO("Construct Navigator\r\n");

    mMainState = NMS_IDLE;
    mThreadRun = true;

    auto thread_navi = std::make_unique<std::thread> (&Navigator::naviThreadEntry, this);
    thread_navi->detach();

    mSimu = std::make_unique<Simu> ("../simulation/house.bmp", "simulation");
    mSimu->registEventCallback(std::bind(&Navigator::listenSimuEvent, this, std::placeholders::_1));

    while (true)
    {
        mSleep(500);
    }
}

/**
 * @brief  Destroy the Navigator:: Navigator object
 * @author Jimmy 
 * @date   2021.09.01
 */
Navigator::~Navigator()
{
    log_shell::LOG_INFO("Destroy Navigator\r\n");
    mThreadRun = false;
}

/**
 * @brief  路径规划线程
 * @author Jimmy 
 * @date   2021.09.01
 */
void Navigator::naviThreadEntry(void)
{
    while (mThreadRun)
    {
        switch (mMainState)
        {
        case NMS_IDLE:
        {
            break;
        }
        case NMS_TARGET_PATHING:
        {
            log_shell::LOG_INFO("Target path finding\r\n");
            MapData mapData;
            std::vector<Point> path;
            path.clear();
            mSimu->getMap(mapData);
            Point startPoint, endPoint;
            startPoint = mSimu->getStartPoint();
            endPoint   = mSimu->getEndPoint();
            
            auto ast = std::make_unique<AStar> ();
            if (mapData.param.width < 1000 || mapData.param.height < 1000)
            {
                ast->updateMap(mapData);
            }
            else
            {
                ast->updateMap(mapData, mapData.param.width, mapData.param.height, 0, 0, mapData.param.width, mapData.param.height, true);
            }
            
            if (ast->pathing(startPoint, endPoint, path) == a_star_normal::ASS_OK)
            {
                mSimu->pubPath(path);
            }
            else
            {
                log_shell::LOG_ERROR("AStar pathing failed\r\n");
            }          
            
            mMainState = NMS_IDLE;
            break;
        }
        default:
        {
            mMainState = NMS_IDLE;
        }
        }
        
        mSleep(50);
    }
}

/**
 * @brief  监听仿真界面事件
 * @param  event            
 * @author Jimmy 
 * @date   2021.09.08
 */
void Navigator::listenSimuEvent(sim_shell::sim_event_t event)
{
    log_shell::LOG_INFO("callback :%d\r\n", event);

    switch (event)
    {
    case sim_shell::SME_NULL:
        /* code */
        break;
    case sim_shell::SME_PATHING:
        mMainState = NMS_TARGET_PATHING;
        break;
    default:
        break;
    }
}



/* End of file */
